#include "feature/feature.h"

using namespace feature;

Feature::Feature() {

}

Feature::Feature(const uint8_t& camera_id, const uint64_t& frame_id, const uint64_t& feature_id, const FeaturePoint& feature) {
	feature_id_ = feature_id;
    map_camera_id_and_feature_[camera_id].push_back(feature);
    map_camera_and_frame_id_[camera_id].push_back(frame_id);
}

std::unique_ptr<Vector3d> Feature::GetNormalizedPointFromCameraIdAndIndex(const uint8_t& camera_id, const uint32_t& point_idx) {
    std::unique_ptr<Vector3d> point_ptr = nullptr;

    std::vector<FeaturePoint> feature_points = this->GetFeaturePointFromCameraId(camera_id);
    if(!feature_points.empty()) {
        point_ptr = common::make_unique<Vector3d>(feature_points[point_idx].normalized_point);
    }

    return point_ptr;
}

std::vector<FeaturePoint> Feature::GetFeaturePointFromCameraId(const uint8_t& camera_id) {
    std::vector<FeaturePoint> feature_points;
    auto feature_points_iter = map_camera_id_and_feature_.find(camera_id);
    if(feature_points_iter != map_camera_id_and_feature_.end()) {
        feature_points = feature_points_iter->second;
    }   
    return feature_points;
}

std::vector<uint64_t> Feature::GetFrameIdsFromCameraId(const uint8_t& camera_id) {
    std::vector<uint64_t> frame_ids;

    auto frame_ids_iter = map_camera_and_frame_id_.find(camera_id);
    if(frame_ids_iter != map_camera_and_frame_id_.end()) {
        frame_ids = frame_ids_iter->second;
    }   

    return frame_ids;
}